//File: MatrixMath.cpp
#include "MatrixMath.h"
#include "Utility.h"
#include <cmath>

// |------- class Vec3 -------|

Vec3::Vec3()
{
	Fill(0.0f);
}
Vec3::Vec3(float Fill_Value)
{
	Fill(Fill_Value);
}
Vec3::Vec3(float x, float y, float z)
{
	X = x;
	Y = y;
	Z = z;
}

bool Vec3::operator == (Vec3 V)
{
	if (X == V.X && Y == V.Y && Z == V.Z)
		return true;

	return false;
}

Vec3 Vec3::operator * (float Scalar)
{
	Vec3 Ret = *this;
	Ret.X *= Scalar;
	Ret.Y *= Scalar;
	Ret.Z *= Scalar;
	return Ret;
}
Vec3 Vec3::operator + (float Scalar)
{
	Vec3 Ret = *this;
	Ret.X += Scalar;
	Ret.Y += Scalar;
	Ret.Z += Scalar;
	return Ret;
}
Vec3 Vec3::operator - (float Scalar)
{
	Vec3 Ret = *this;
	Ret.X -= Scalar;
	Ret.Y -= Scalar;
	Ret.Z -= Scalar;
	return Ret;

}
Vec3 Vec3::operator / (float Scalar)
{
	Vec3 Ret = *this;
	Ret.X /= Scalar;
	Ret.Y /= Scalar;
	Ret.Z /= Scalar;
	return Ret;
}

Vec3 Vec3::operator * (Vec3 Vec)
{
	Vec3 Ret = *this;
	Ret.X *= Vec.X;
	Ret.Y *= Vec.Y;
	Ret.Z *= Vec.Z;
	return Ret;
}
Vec3 Vec3::operator + (Vec3 Vec)
{
	Vec3 Ret = *this;
	Ret.X += Vec.X;
	Ret.Y += Vec.Y;
	Ret.Z += Vec.Z;
	return Ret;
}
Vec3 Vec3::operator - (Vec3 Vec)
{
	Vec3 Ret = *this;
	Ret.X -= Vec.X;
	Ret.Y -= Vec.Y;
	Ret.Z -= Vec.Z;
	return Ret;

}
Vec3 Vec3::operator / (Vec3 Vec)
{
	Vec3 Ret = *this;
	Ret.X /= Vec.X;
	Ret.Y /= Vec.Y;
	Ret.Z /= Vec.Z;
	return Ret;
}


void	Vec3::Fill(float Fill_Value)
{
	X = Y = Z = Fill_Value;
}
float	Vec3::Dot(Vec3 Vector)
{
	// Dot product = ||A|| * ||B|| * Cos(Theta)
	// Dot product = How similar the direction of the two vectors are.
	// If Dot = 0, then the vectors are orthogonal, or 90 degrees from each other.

	return (float) X*Vector.X + Y*Vector.Y + Z*Vector.Z;
}
float	Vec3::Length()
{	
	// Square root of the dot product of a vector with itself is the vector's length.
	//return (float) sqrt(pow(X,2) + pow(Y,2) + pow(Z,2));

	return (float) sqrt(Dot(*this));
}

Vec3	Vec3::CrossProduct(Vec3 Vec)
{
	// Creates a vector orthogonal (90 degrees) to the two multiplied vectors.
	// Where the dot product of the two vectors are equal to zero.

		// Length of Cross product = ||A|| * ||B|| * Sin(Theta)  ... where ||X|| = X.Length();
		// Length of Cross product: A measure of how perpendictular or orthogonal two vectors are.


	//a2*b3-a3*b2 (Ignore row 1) (Top->Bottom - Bottom->Tom)
	//a3*b1-a1*b3 (Ignore row 2) ("Inverse cross multiply" Bottom->Top - Top->Bottom)
	//a1*b2-a2*b1 (Ignore row 3) (Top->Bottom - Bottom->Tom)
	Vec3 Ret;
	Ret.X = Y*Vec.Z-Z*Vec.Y;
	Ret.Y = Z*Vec.X-X*Vec.Z;
	Ret.Z = X*Vec.Y-Y*Vec.X;

	return Ret;
}
float	Vec3::AngleBetween(Vec3 V)
{
	// Get the angle between two vectors.
	// Read as: Theta = ArcCos of Dot(This and V) divided by (This and V)'s lengths.
	return acos(Dot(V)/ (Length()*V.Length()) );
}
bool	Vec3::IsOrthogonal(Vec3 V)
{
	// Returns true if the two vectors are perpendicular
	//or...  if (this->CrossProduct(V).Length() == 0) ... this is also valid.
	if (this->Dot(V) == 0) {return true;}
	return false;

}
bool	Vec3::IsColinear(Vec3 V)
{
	// Return true if both vectors trace or are on top of eachother.
	if (this->CrossProduct(V).Length() == 0)
		return true;

	return false;
}
Vec3	Vec3::Normalized()
{
	return ((Vec3)*this) / Length();
}
Vec3	Vec3::Unit()
{
	return Normalized();
}

void Vec3::Cout_Print()
{
	std::cout << X << " " << Y << " " << Z << std::endl;
}




// |------- class Mat4 -------|
Mat4::Mat4()
{
	Fill(0);
	//std::cout << "Row1\n";
	//std::cout << Mat[0][0] << " " << Mat[0][1] << " " << Mat[0][2] << " "<< Mat[0][3] << std::endl;
}
Mat4::~Mat4()
{

}

void Mat4::Fill(float Val)
{
	//memset(Mat, 0, sizeof(float) * 16);

	for (int A = 0; A < 4;A++)
	{
		for (int B = 0; B < 4;B++)
		{
			Mat[A][B] = Val;
		}
	}
}
void Mat4::Fill_Addition(float StartingNumber, float Increment)
{
	for (int A = 0; A < 4; A++)
	{
		for (int B = 0; B < 4; B++)
		{
			Mat[A][B] = StartingNumber;
			StartingNumber += Increment;
		}
	}
}

void Mat4::Transform(Vec3 _Vector)
{
	Identity();
	Mat[0][3] = _Vector.X;
	Mat[1][3] = _Vector.Y;
	Mat[2][3] = _Vector.Z;
	Mat[3][3] = 1.0f;
}
void Mat4::SetPos(Vec3 _Vector)
{
	Mat[0][3] = _Vector.X;
	Mat[1][3] = _Vector.Y;
	Mat[2][3] = _Vector.Z;
}

void Mat4::Rotate(Vec3 _Angle)
{
	if (_Angle.X != 0)
		Rotate_X(_Angle.X);

	if (_Angle.Y != 0)
		Rotate_Y(_Angle.Y);

	if (_Angle.Z > 0)
		Rotate_Z(_Angle.Z);
}
void Mat4::Rotate_X(float Degree)
{
	Mat[1][1] = cos(Degree);
	Mat[1][2] = -sin(Degree);
	Mat[2][1] = sin(Degree);
	Mat[2][2] = cos(Degree);
}
void Mat4::Rotate_Y(float Degree)
{
	Mat[0][0] = cos(Degree);
	Mat[0][2] = sin(Degree);
	Mat[2][0] = -sin(Degree);
	Mat[2][2] = cos(Degree);
}
void Mat4::Rotate_Z(float Degree)
{
	Mat[0][0] = cos(Degree);
	Mat[0][1] = -sin(Degree);
	Mat[1][1] = sin(Degree);
	Mat[1][2] = cos(Degree);
}

void Mat4::Identity()
{
	Fill(0);
	Mat[0][0] = 1;
	Mat[1][1] = 1;
	Mat[2][2] = 1;
	Mat[3][3] = 1;



}
void Mat4::Identity_Perspective()
{
	Identity();
	Mat[2][2] = -1;

	//Set W to 1.
	Mat[3][0] = 1;
	Mat[3][1] = 1;
	Mat[3][2] = 1;
}


float* Mat4::SetMat16()
{

	int Matrix16_Index = 0;

	for (int A = 0; A < 4; A++)
	{
		for (int B = 0; B < 4; B++)
		{
			//std::cout << "Buffer["<<Matrix16_Index<<"] =" << " Mat["<<A<<"]["<<B<<"] " << " = " << Mat[A][B] << " \n";

			Mat16[Matrix16_Index] = Mat[A][B];
			Matrix16_Index++;
		}
	}
	return Mat16;
}


std::array<float,16> Mat4::Get_Mat16()
{
	/* It was either requesting a buffer of 16 floats and hoping not to
		overflow into other memory spaces, or declaring a 'new' dynamic variable.
		The hell if I'm going to let the stack get corrupted or 
		use delete every time I call this function.	*/

	std::array<float,16> Buffer;
	int Matrix16_Index = 0;

	for (int A = 0; A < 4; A++)
	{
		for (int B = 0; B < 4; B++)
		{
			std::cout << "Buffer["<<Matrix16_Index<<"] =" << " Mat["<<A<<"]["<<B<<"]\n";

			Buffer[Matrix16_Index] = Mat[A][B];
			Matrix16_Index++;
		}
	}
	return Buffer;
}

void Mat4::Cout_Print()
{
	for (int A = 0; A < 4; A++)
	{
		for (int B = 0; B < 4; B++)
		{
			std::cout << Mat[A][B] << "\t";
		}
		std::cout << std::endl;
	}

}

void Mat4::Scale(Vec3 Scale)
{
	Mat[0][0] *= Scale.X;
	Mat[1][1] *= Scale.Y;
	Mat[2][2] *= Scale.Z;
}
void Mat4::SetScale(Vec3 Scale)
{
	Mat[0][0] = Scale.X;
	Mat[1][1] = Scale.Y;
	Mat[2][2] = Scale.Z;
}

void Mat4::Orthographic(float Left, float Right, float Bottom, float Top, float Near, float Far)
{
	Identity();
    Mat[0][0]  = 2 / (Right - Left);
    Mat[0][2]  = 2 / (Top - Bottom);
    Mat[1][1]  = -2 / (Far - Near);
    Mat[0][3]  = (Right + Left) / (Right - Left);

    Mat[1][3] = (Top + Bottom) / (Top - Bottom);
    Mat[2][3] = - ( (Far + Near) / (Far - Near) );
    Mat[3][3] = 1;
}


void Mat4::Perspective(float fovy, float aspect, float zNear, float zFar)
{
	float tanFovy = tan(LinearMath::DegreeToRad(fovy) / 2);
	
	Identity_Perspective();

    Mat[0][0]  = aspect * tanFovy;
    Mat[1][1]  = tanFovy;
    Mat[2][2]  = - (zFar + zNear) / (zFar - zNear);
    Mat[2][3] = -1;
    Mat[3][2] = - (2 * zFar * zNear) / (zFar - zNear);

	SetMat16();

	/*

		template <typename valType> 
	GLM_FUNC_QUALIFIER detail::tmat4x4<valType> perspective
	(
		valType const & fovy, 
		valType const & aspect, 
		valType const & zNear, 
		valType const & zFar
	)
	{
		assert(aspect != valType(0));
		assert(zFar != zNear);

#ifdef GLM_FORCE_RADIANS
		valType const rad = fovy;
#else
		valType const rad = glm::radians(fovy);
#endif

		valType tanHalfFovy = tan(rad / valType(2));
		detail::tmat4x4<valType> Result(valType(0));
		Result[0][0] = valType(1) / (aspect * tanHalfFovy);
		Result[1][1] = valType(1) / (tanHalfFovy);
		Result[2][2] = - (zFar + zNear) / (zFar - zNear);
		Result[2][3] = - valType(1);
		Result[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear);
		return Result;
	}
	*/
}


void Mat4::SetMatrix(Mat4 NewMatrix)
{
	memcpy(Mat,NewMatrix.Mat,sizeof(Mat));
}

Mat4 Mat4::Multiply(Mat4 Mult)
{			
	/* God help us, I made this and even I am having a hard time reading it 
		Methinks there's a better way.
		 ... The lady doth protest too much, methinks. */

	Mat4 New;

	int Row = 0;
	int Col = 0;
	float Final = 0.0f;

	for (int ProcessCol = 0; ProcessCol < 4; ProcessCol++)
	{
		Col = 0;
		for (int ProcessRow = 0; ProcessRow < 4; ProcessRow++)
		{
			Final = 0.0f;
			for(int Proc = 0; Proc < 4;Proc++)
			{
				Final += Mat[Row][Proc]*Mult.Mat[Proc][Col];
			}
			New.Mat[Row][Col] = Final;
			Col++;
		}
		Row++;
	}
	return New;
}

Mat4 Mat4::operator*(Mat4 Mult)
{
	return Multiply(Mult);
};
void Mat4::operator=(Mat4 M)
{
	SetMatrix(M);
};



float LinearMath::DegreeToRad(float Degree)
{
	return Degree * ( PI_F / 180);
}